#include "TargetingHelper.h"
#include "PDController.h"
#include "AttractiveField.h"

World* world = World::instance();
vector<TangentField> TargetingHelper::tangent_fields;
bool TargetingHelper::initialized = false;

TargetingHelper::TargetingHelper(void)
{
}


TargetingHelper::~TargetingHelper(void)
{
}

Command TargetingHelper::Target(tank_t* tank, KalmanTank* enemy) {

	if (!initialized) {
		initialized = true;

		for(int i = 0; i < world->obstacles().size(); i ++){
			KalmanObstacle* obs = &world->kobstacles().at(i);
			tangent_fields.push_back(TangentField(obs));
		}
	}

	float tthreshold = M_PI / 8.0f;
	float shot_threshold = 0.015;
	Vector2 tank_pos(tank->pos[0], tank->pos[1]);
	Vector2 aim = enemy->Predict(calcShot(tank, enemy));

	aim = enemy->GetCurrentEstimate();
	
	Vector2 aim_traj = aim - tank_pos;
	if(aim_traj.magnitude() <= World::instance()->getShotRange()) {
		float dtheta = aim_traj.angle() - PDController::instance()->normalizeAngle(tank->angle);
		if(dtheta > M_PI)
			dtheta -= 2.0f*M_PI;
		else if(dtheta < -M_PI)
			dtheta += 2.0f*M_PI;
	}

	AttractiveField afield(aim);
	Vector2 trajectory = afield.get_trajectory(tank_pos);

	/*for (unsigned int i = 0; i < tangent_fields.size(); i++) {
		
		Vector2 tang_trajectory = tangent_fields[i].get_trajectory(tank_pos);
		
		if (tang_trajectory.magnitude() > 0) {
			trajectory += tang_trajectory.normalize() * 4.0f;// * (world->getSize() / 2.0f);
		}
	}*/

	Command cmd = PDController::instance()->GetCommand(tank, trajectory);
	
	bool shot = true;
	if (fabs(cmd.angvel()) <= shot_threshold) {
		shot = true;
	}
	cmd.shoot(shot);
	
	return cmd;
}

float TargetingHelper::calcShot(tank_t* tank, KalmanTank* enemy) {

	float t = 0.0;
	Vector2 tankP = Vector2(tank->pos[0], tank->pos[1]);
	Vector2 predicted;
	float shotV = World::instance()->getBulletSpeed();
	float shotT = 0.0;

	while(1){
		t += PREDICT_STEP;
		predicted = enemy->Predict(t);
		shotT = (predicted-tankP).magnitude()/shotV;
		if(fabs(shotT-t) < EPSILON || 
		   fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
		   fabs(predicted.y) * 2.0f > World::instance()->getSize())
			return t;
	}
}